package com.sarc.vision;

//import java.util.Vector;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;

import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import android.util.Log;

public class PTAMSystem extends Thread 
{
	State currentState = State.NOT_INITIALIZED;
	NativeState nativeState = NativeState.NOT_INITIALIZED;
	NativeState nativeStateCompre = NativeState.NOT_INITIALIZED;
	NativeState lastNativeState = NativeState.NOT_INITIALIZED;
	
	
	long frameCounter = 0;
	boolean newFrameAvailable, rocessedFrameAvailable;
	boolean showProcessing;
	boolean shouldRun;
	boolean running;
	boolean readyForFrame = false;
	boolean shouldTerminate = false;
	boolean useNativeCamera = false;
	
	Mat camFrameBuffer;
	Mat grayCamFrame;
	Mat processedFrame;
	Mat outFrameBuffer;
	
	Lock dataProcessingLock;
	Lock frameTransferringLock;
	
	OnCalibrationCompletedListener onCalibrationCompletedListener = null;
	OnTrackingInitCompletedListener onTrackingInitCompletedListener = null; 
	OnFrameUpdatedListener onFrameUpdatedListener = null;
	
	private static final String TAG = "SARC:PTAM"; 

	public PTAMSystem(boolean useNativeCamera)
	{
		this(false, useNativeCamera);
	}
	
	public PTAMSystem(boolean showProcessing, boolean useNativeCamera)
	{
		this.useNativeCamera = useNativeCamera;
		this.showProcessing = showProcessing;
		dataProcessingLock = new ReentrantLock();
		frameTransferringLock = new ReentrantLock();
		Log.d(TAG, "PTAM System (" + Integer.valueOf(this.hashCode()).toString() + ") started");
		if (useNativeCamera)
		{
			//System.loadLibrary("libnative_camera_r2.2.0");
			camFrameBuffer = new Mat(new Size(640,480),  CvType.CV_8UC4);
			grayCamFrame = new Mat(camFrameBuffer.size(), CvType.CV_8UC1); 
			processedFrame = new Mat(camFrameBuffer.size(), CvType.CV_8UC4);
			outFrameBuffer = new Mat(camFrameBuffer.size(), CvType.CV_8UC4);
			readyForFrame = true;
		}
	}
		
	public void initialize()
	{	
		nativeState = NativeState.valueOf(NativeInit());
		Log.d(TAG, "PTAM System (" + Integer.valueOf(this.hashCode()).toString() + ") Initialized without camera calibration matrix");
	}
	
	public void initialize(Mat cameraMatrixMat, Mat distortionCoefMat)
	{
		nativeState = NativeState.valueOf(NativeInitWithCamParam(cameraMatrixMat.nativeObj, distortionCoefMat.nativeObj));
		Log.d(TAG, "PTAM System (" + Integer.valueOf(this.hashCode()).toString() + ") initialised with camera calibration matrix");
	}
	
	protected void finalize()
	{
		cleanup();
	}
	
	public void cleanup() {
		// TODO Auto-generated method stub
		nativeState = NativeState.valueOf(NativeRelease());
		Log.d(TAG, "PTAM System (" + Integer.valueOf(this.hashCode()).toString() + ") released");
	}

	public void run()
	{		
		shouldRun = true;
		
		Log.d(TAG, "Parser thread started and running");
		
		while (!shouldTerminate)
		{
			dataProcessingLock.lock();
			running = true;
			dataProcessingLock.unlock();
			
			currentState = State.NOT_RUNNING;
			
			while (shouldRun)
			{
				dataProcessingLock.lock();
				try
				{
					if (useNativeCamera || newFrameAvailable)
					{
						if (!useNativeCamera)
						{
							//Get new Frame Here
							frameTransferringLock.lock();
							try
							{
								Imgproc.cvtColor(camFrameBuffer, grayCamFrame, Imgproc.COLOR_BGRA2GRAY);
								camFrameBuffer.copyTo(processedFrame);
							}
							finally
							{
								frameTransferringLock.unlock();
							}
						}
						
						lastNativeState = nativeState;
						nativeState = NativeState.valueOf(NativeProcessNewFrame(grayCamFrame.nativeObj, processedFrame.nativeObj));
						
						
						if (nativeState.index() >= 0)
						{	
							currentState = State.RUNNING;
							if (nativeState != NativeState.FRAME_SKIPPED)
							{
								nativeStateCompre = nativeState;
							}
							frameTransferringLock.lock();
							
							try
							{
								if (showProcessing)
								{
									processedFrame.copyTo(outFrameBuffer);
								}
								else
								{
									camFrameBuffer.copyTo(outFrameBuffer);
								}
								
								if (onFrameUpdatedListener != null)
								{
									onFrameUpdatedListener.OnFrameUpdated(outFrameBuffer, nativeStateCompre);
								}
							}
							finally
							{
								frameTransferringLock.unlock();
							}
						}
						else
						{
							currentState = State.ERROR;
						}
												
						if (nativeState != lastNativeState)
						{
							switch (nativeState)
							{
								case CALIBRATION_COMPLETED:
									if (onCalibrationCompletedListener != null)
									{
										Mat camMatrix = new Mat(3,3, CvType.CV_64F);
										Mat distCoef = new Mat(5,1,CvType.CV_64F);
										NativeGetCameraMatrix(camMatrix.nativeObj, distCoef.nativeObj);
										onCalibrationCompletedListener.OnCalibrationCompleted(camMatrix, distCoef);
										camMatrix.release();
										distCoef.release();
									}
									break;
								case TRACKING:
									if (onTrackingInitCompletedListener != null)
									{
										
										Mat rv = new Mat(1,3,CvType.CV_32F);
										Mat tv = new Mat(1,3,CvType.CV_32F);
										Mat uv = new Mat(1,3,CvType.CV_32F);
										Mat vv = new Mat(1,3,CvType.CV_32F);
										
										NativeGetExtrinsics(rv.nativeObj, tv.nativeObj);
										NativeGetUV(uv.nativeObj, vv.nativeObj);
										onTrackingInitCompletedListener.OnTrackingInitCompleted(rv, tv, uv, vv);
										
										rv.release();
										tv.release();
										uv.release();
										vv.release();
									}
								case ERROR:
									Log.e(TAG, "OMG there's an error!");
								default: Log.d(TAG, "State change from " + lastNativeState.toString() + " to " + nativeState.toString());;
							}
						}
						
						newFrameAvailable = false;
						
					}
				}
				catch(Exception ex)
				{
					ex.printStackTrace();
				}
				finally
				{
					dataProcessingLock.unlock();
				}				
			}
			
			
			dataProcessingLock.lock();
			running = false;
			dataProcessingLock.unlock();
		}
	}
	
	public void pauseRunning()
	{
		dataProcessingLock.lock();
		shouldRun = false;
		dataProcessingLock.unlock();
	}
	
	public void resumeRunning()
	{
		dataProcessingLock.lock();
		shouldRun = true;
		dataProcessingLock.unlock();
	}
	
	public void stopRunning() throws InterruptedException
	{
		dataProcessingLock.lock();
		shouldRun = false;
		shouldTerminate = true;
		dataProcessingLock.unlock();
		join();
	}
	
	public boolean isRunning()
	{

		dataProcessingLock.lock();
		boolean result = running;
		dataProcessingLock.unlock();
		return result;
	}
	
	public void captureOnNextFrame()
	{
		dataProcessingLock.lock();
		if (nativeState != NativeState.FRAME_SKIPPED)
		{
			NativeCaptureOnNextFrame();
		}
		dataProcessingLock.unlock();
	}
	
	public void resetOnNextFrame()
	{
		dataProcessingLock.lock();
		NativeResetOnNextFrame();
		dataProcessingLock.unlock();
	}
	
	public boolean getCameraExtrinsics(Mat rv, Mat tv)
	{
		boolean success = false;
		dataProcessingLock.lock();
		try
		{
			success = NativeGetExtrinsics(rv.nativeObj, tv.nativeObj);
		}
		finally
		{
			dataProcessingLock.unlock();
		}
		return success;
	}
	
	public boolean getReferencePoint(Mat refPt)
	{
		boolean success = false;
		dataProcessingLock.lock();
		try
		{
			success = NativeGetReferencePoint(refPt.nativeObj);
		}
		finally
		{
			dataProcessingLock.unlock();
		}
		return success;
	}
	
	public NativeState getNativeCurrentState()
	{
		dataProcessingLock.lock();
		NativeState returnState = nativeState;
		dataProcessingLock.unlock();
		return returnState;
	}
	
	public State getCurrentState()
	{
		dataProcessingLock.lock();
		State returnState =  currentState;
		dataProcessingLock.unlock();
		return returnState;
	}
	
	public void setShouldWriteMessage(boolean shouldWriteMessage)
	{
		dataProcessingLock.lock();
		NativeSetShouldWriteMessage(shouldWriteMessage);
		dataProcessingLock.unlock();
	}

	public void pushNewFrame(Mat inputFrame) {
		// TODO Auto-generated method stub
		
		if (!readyForFrame)
		{
			
			camFrameBuffer = new Mat(inputFrame.size(),  CvType.CV_8UC4);
			grayCamFrame = new Mat(inputFrame.size(), CvType.CV_8UC1); 
			processedFrame = new Mat(inputFrame.size(), CvType.CV_8UC4);
			outFrameBuffer = new Mat(inputFrame.size(), CvType.CV_8UC4);
			readyForFrame = true;
		}
		
		frameTransferringLock.lock();
		try
		{
			//Log.d(TAG,"F->" + Boolean.valueOf(newFrameAvailable).toString());
			inputFrame.copyTo(camFrameBuffer);
			
			frameCounter++;
			newFrameAvailable = true;
		}
		finally
		{
			frameTransferringLock.unlock();
		}
	}
	
	public void retrieveFromOutputFrame(Mat retriever)
	{
		frameTransferringLock.lock();
		try
		{
			if (showProcessing)
			{
				outFrameBuffer.copyTo(retriever);
			}
			else 
			{
				camFrameBuffer.copyTo(retriever);
			}
		}
		finally
		{
			frameTransferringLock.unlock();
		}
		
	}
	
	public void setOnTrackingInitCompletedListener(OnTrackingInitCompletedListener listener)
	{
		this.onTrackingInitCompletedListener = listener;
	}
	
	public void setOnCalibrationCompletedListener(OnCalibrationCompletedListener listener)
	{
		this.onCalibrationCompletedListener = listener;
	}
	
	public void setOnFrameUpdatedListener(OnFrameUpdatedListener listener)
	{
		this.onFrameUpdatedListener = listener;
	}
	
	//definition from NativePTAMSystem	
	public enum NativeState {
		ERROR								(-1),
		NOT_INITIALIZED						(-2),
		NOT_CALIBRATED						(-3),
		
		FRAME_SKIPPED						(0),
		INITIALIZED_WITH_CAM_PARAM			(1),
		INITIALIZED_WITHOUT_CAM_PARAM		(2),
		CALIBRATING_CAMERA					(3),
		CALIBRATION_COMPLETED				(4),
		STARTING_TRACKING_INIT				(5),
		TRACKING_INIT_NEW_FRAME				(6),
		TRACKING_INIT_FINISHED				(7),
		TRACKING							(8),
		TESTING_STATE						(777);

	    private final int index;   

	    NativeState (int index) {
	        this.index = index;
	    }

	    public int index() { 
	        return index; 
	    }
	    
	    public static NativeState valueOf(int index) 
	    {
	    	switch (index) {
	    	case -1:
	    		return NativeState.ERROR;
	    	case -2:
	    		return NativeState.NOT_INITIALIZED;
	    	case -3:
	    		return NativeState.NOT_CALIBRATED;
	    	case 0:
	    		return NativeState.FRAME_SKIPPED;
	    	case 1:
	    		return NativeState.INITIALIZED_WITH_CAM_PARAM;
	    	case 2:
	    		return NativeState.INITIALIZED_WITHOUT_CAM_PARAM;
	    	case 3:
	    		return NativeState.CALIBRATING_CAMERA;
	    	case 4:
	    		return NativeState.CALIBRATION_COMPLETED;
	    	case 5:
	    		return NativeState.STARTING_TRACKING_INIT;
	    	case 6:
	    		return NativeState.TRACKING_INIT_NEW_FRAME;
	    	case 7:
	    		return NativeState.TRACKING_INIT_FINISHED;
	    	case 8:
	    		return NativeState.TRACKING;
	    	case 777:
	    		return NativeState.TESTING_STATE;

	    	default:
	    		throw new RuntimeException("Unknown index:" + index);
	    	}
	    }
	}
	
	public enum State 
	{
		NOT_INITIALIZED,NOT_RUNNING, RUNNING, ERROR;
	}
		
	public interface OnCalibrationCompletedListener
	{
		public void OnCalibrationCompleted(Mat CameraMatrix, Mat DistortionCoefficients);
	}
	
	public interface OnTrackingInitCompletedListener
	{
		public void OnTrackingInitCompleted(Mat rv, Mat tv, Mat u, Mat v);
	}
	
	public interface OnFrameUpdatedListener
	{
		public void OnFrameUpdated(Mat frame, NativeState ptamState);
	}
		
	private native int NativeInit();
	private native int NativeInitWithCamParam(long addrCameraMatrixMat, long addrDistortionCoefMat);
	private native int NativeRelease();
	
	private native boolean NativeSetShouldWriteMessage(boolean shouldWriteMessage);
	private native boolean NativeCaptureOnNextFrame();
	private native boolean NativeResetOnNextFrame();
	private native boolean NativeGetExtrinsics(long addrMatRv, long addrMatTv);
	private native boolean NativeGetUV(long addrMatUV, long addrMatVV);
	private native boolean NativeGetReferencePoint(long addrRefPt);
	private native boolean NativeGetCameraMatrix(long addrCameraMatrixMat, long addrDistortionCoefMat);
	private native String NativeGetOutputString();
	private native int NativeProcessNewFrame(long addrGray, long addrFrameToDraw);
	
}
